#include <librealsense2/rs.hpp>
#include <open3d/Open3D.h>
#include <opencv2/opencv.hpp>
#include <thread>

using namespace open3d;
using namespace std;
using namespace cv;

int main(int argc, char *argv[])
{
    auto dep = imread("/home/mini/图片/dep.png", IMREAD_ANYDEPTH);
    geometry::Image color, depth;
    io::ReadImage("/home/mini/图片/rgb.png", color);
    io::ReadImage("/home/mini/图片/dep.png", depth);

    auto rgbd = geometry::RGBDImage::CreateFromColorAndDepth(color, depth, 1000.0, 4.0, false);

    auto intrinsic = camera::PinholeCameraIntrinsic(1280, 720, 646.723755, 350.124908, 636.619141, 636.619141);
    auto pcd = geometry::PointCloud::CreateFromRGBDImage(*rgbd, intrinsic);
    Eigen::Matrix4d m(4, 4);
    m << 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1;
    pcd->Transform(m);

    visualization::Visualizer visualizer;

    visualizer.CreateVisualizerWindow("Open3D", 1280, 720);
    visualizer.AddGeometry(pcd);
    visualizer.Run();
}

// std::this_thread::sleep_for(std::chrono::milliseconds(50));

// auto depth_image_ptr = std::make_shared<geometry::Image>();
// depth_image_ptr->Prepare(640, 480, 1, 2);
// auto color_image_ptr = std::make_shared<geometry::Image>();
// color_image_ptr->Prepare(640, 480, 3, 1);

// visualization::Visualizer depth_vis, color_vis;
// if (!depth_vis.CreateVisualizerWindow("Depth", 640, 480, 15, 50) ||
//     !depth_vis.AddGeometry(depth_image_ptr) ||
//     !color_vis.CreateVisualizerWindow("Color", 640, 480, 675, 50) ||
//     !color_vis.AddGeometry(color_image_ptr))
// {
//     return 0;
// }

// utility::FPSTimer timer("Realsense stream");
// rs2::align al(RS2_STREAM_COLOR);

// while (depth_vis.PollEvents() && color_vis.PollEvents()) {
//     timer.Signal();
//     auto formets = p.wait_for_frames();

//     auto alignedSet = al.process(formets);

//     auto rgbdata =  alignedSet.get_color_frame();
//     auto depthdata = alignedSet.get_depth_frame();

//     memcpy(depth_image_ptr->data_.data(),
//            (void *)depthdata.get_data(), 640 * 480 * 2);
//     memcpy(color_image_ptr->data_.data(),
//            (void *)rgbdata.get_data(),
//            640 * 480 * 3);

//     depth_vis.UpdateGeometry();
//     color_vis.UpdateGeometry();
// }